package com.neuronrobotics.replicator.gui.preview.android;

import java.util.ArrayList;
import java.util.Iterator;

import javax.vecmath.Vector3f;

import com.neuronrobotics.replicator.gui.stl.STLFace;
import com.neuronrobotics.replicator.gui.stl.STLObject;

public class PickingRay {
	private Vector3f clickPosInWorld = new Vector3f();
	private Vector3f direction = new Vector3f();

	/**
	 * Computes the intersection of this ray with the X-Y Plane (where Z =
	 * 0) and writes it back to the provided vector.
	 */
	public void intersectionWithXyPlane(float[] worldPos) {
		float s = -clickPosInWorld.z / direction.z;
		worldPos[0] = clickPosInWorld.x + direction.x * s;
		worldPos[1] = clickPosInWorld.y + direction.y * s;
		worldPos[2] = 0;
	}

	/**
	 * Computes the intersection of this ray with the X-Z Plane (where Y =
	 * 0) and writes it back to the provided vector.
	 */
	public void intersectionWithXzPlane(float[] worldPos) {
		float s = -clickPosInWorld.y / direction.y;
		worldPos[0] = clickPosInWorld.x + direction.x * s;
		worldPos[1] = 0;
		worldPos[2] = clickPosInWorld.z + direction.z * s;
	}

	public Vector3f getClickPosInWorld() {
		return clickPosInWorld;
	}

	public Vector3f getDirection() {
		return direction;
	}
	
	//TODO may need more information to do this better
	//possibly not the best place for this
	//currently just compares and assumes an itersection
	public STLFace getPickFace(STLObject stl, float[] currTran){
		Iterator<STLFace> currFaceIterator = stl.getFaceIterator();
		STLFace thePick = null;
		
		ArrayList<STLFace> faces = new ArrayList<STLFace>();
		ArrayList<Float> angles = new ArrayList<Float>();
		
		while(currFaceIterator.hasNext()){
			STLFace currCand = currFaceIterator.next();
			
			float[] currNorm = new float[4];
			currCand.getNormal(currNorm, 0);
			currNorm[3] = 1f;
			
			float ang = this.direction.angle(new Vector3f(currNorm));
			faces.add(currCand);
			angles.add(ang);		
			
		}
		
		float currAng = Float.NEGATIVE_INFINITY;
		int ct=0;
		for(STLFace currF:faces){
			if(angles.get(ct)>currAng){
				thePick = currF;
				currAng = angles.get(ct);
			}
			ct++;
		}
		
		return thePick;
	}
	
	public static class PickUtilities{
		
		public static boolean raySphereIntersection(Vector3f or, Vector3f dir,
				Vector3f cent, float radius) {

			float a, b, c;
			a = dir.x * dir.x + dir.y * dir.y + dir.z * dir.z;
			b = 2 * (dir.x * (or.x - cent.x) + dir.y * (or.y - cent.y) + dir.z
					* (or.z - cent.z));
			c = (or.x - cent.x) * (or.x - cent.x) + (or.y - cent.y)
					* (or.y - cent.y)
					+ ((or.z - cent.z) * (or.z - cent.z) - radius * radius);

			float disc = b * b - 4 * a * c;

			if (disc < 0)
				return false;

			float t0 = (float) (-b - Math.sqrt(b * b - 4 * c) / 2.0f);
			float t1 = (float) (-b + Math.sqrt(b * b - 4 * c) / 2.0f);
			return (t0 >= 0 || t1 >= 0);

		}

		public static boolean raySphereIntersection(float[] or, float[] dir, float[] cent,
				float radius) {

			float a, b, c;
			a = dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2];
			b = 2 * (dir[0] * (or[0] - cent[0]) + dir[1] * (or[1] - cent[1]) + dir[2]
					* (or[2] - cent[2]));
			c = (or[0] - cent[0]) * (or[0] - cent[0]) + (or[1] - cent[1])
					* (or[1] - cent[1])
					+ ((or[2] - cent[2]) * (or[2] - cent[2]) - radius * radius);

			float disc = b * b - 4 * a * c;

			if (disc < 0)
				return false;

			float t0 = (float) (-b - Math.sqrt(b * b - 4 * c) / 2.0f);
			float t1 = (float) (-b + Math.sqrt(b * b - 4 * c) / 2.0f);
			return (t0 >= 0 || t1 >= 0);

		}
		
	}
		
}

